﻿using System;
using System.Collections.Generic;
using AirSimUnity.DroneStructs;
using UnityEditor;
using UnityEngine;

namespace AirSimUnity
{
    /*
     * 用于控制场景中无人机对象的无人机组件。这是基于与AirLib通信的Vehicle类。
     * 该类依赖于基于FastPhysics引擎的AirLib无人机控制器。控制器的使用基于Documents\AirSim中的setting.json文件。
     * 无人机可以通过键盘或客户端API调用进行控制。
     * 这些数据通过PInvoke委托在AirLib和Unity之间不断交换。
     */
    public class Drone : Vehicle
    {
        public Transform[] rotors;
        private List<RotorInfo> rotorInfos = new List<RotorInfo>();
        private float rotationFactor = 0.1f;

        private new void Start()
        {
            base.Start();

            for (int i = 0; i < rotors.Length; i++)
            {
                rotorInfos.Add(new RotorInfo());
            }
        }

        private new void FixedUpdate()
        {
            if (isServerStarted)
            {
                if (resetVehicle)
                {
                    rcData.Reset();
                    currentPose = poseFromAirLib;
                    resetVehicle = false;
                }

                base.FixedUpdate();

                DataManager.SetToUnity(poseFromAirLib.position, ref position);
                DataManager.SetToUnity(poseFromAirLib.orientation, ref rotation);

                transform.position = position;
                transform.rotation = rotation;

                currentPose = poseFromAirLib;

                for (int i = 0; i < rotors.Length; i++)
                {
                    float rotorSpeed = (float)(rotorInfos[i].rotorSpeed * rotorInfos[i].rotorDirection * 180 /
                        Math.PI * rotationFactor);
                    rotors[i].Rotate(Vector3.up, rotorSpeed * Time.deltaTime, Space.Self);
                }
            }
        }

        private new void LateUpdate()
        {
            if (isServerStarted)
            {
                //图像捕获在基类中完成
                base.LateUpdate();
            }
        }



        public KinemticState GetKinematicState()
        {
            return airsimInterface.GetKinematicState();
        }

        #region IVehicleInterface implementation

        // 设置无人机上旋翼的动画。这是通过AirLib通过Pinvoke调用完成的
        public override bool SetRotorSpeed(int rotorIndex, RotorInfo rotorInfo)
        {
            rotorInfos[rotorIndex] = rotorInfo;
            return true;
        }

        // 获取无人机特定的数据，以便在录制时与图像一起保存在文本文件中
        public override DataRecorder.ImageData GetRecordingData()
        {
            DataRecorder.ImageData data;
            data.pose = currentPose;
            data.carData = new CarStructs.CarData();
            data.image = null;
            return data;
        }

        #endregion IVehicleInterface implementation
    }
}